#ifndef __EM_PLANNER_UTILS_HPP__
#define __EM_PLANNER_UTILS_HPP__

#include <iostream>
#include <ros/ros.h>

using namespace std;

#define PI 3.1415926

/**
 * 轨迹坐标点
*/
struct Point {
    float x;
    float y;
    Point() {}
    Point(float x_, float y_) : x(x_), y(y_) {}
};


struct ReferencePoint
{
    double x;           
    double y;           
    double s;           // 弧长
    double heading;     // 航向角（弧度）
    double kappa;       // 曲率
    double dkappa;      // 曲率对弧长的导数（目前还用不到）
    ReferencePoint() {}
    ReferencePoint(double x_, double y_) : x(x_), y(y_) {}
    ReferencePoint(double x_, double y_, double s_, double h_, double k_, double dk_) 
                  : x(x_), y(y_), s(s_), heading(h_), kappa(k_), dkappa(dk_) {}
};


struct SLPoint
{
    double s;
    double s_dot;           // 弧长对时间的导数 ds / dt (速度)
    double s_ddot;          // s_dot 对时间的导数 d(s_dot) / dt (加速度)
    double l;
    double l_prime;         // l 对弧长 s 的导数 dl / ds
    double l_dprime;        // l_prime 对弧长 s 的导数 d(l_prime) / ds
    SLPoint() {}
};



/**
 * 规划输出的轨迹节点数据结构
*/
struct TraPoint {
    double x;
    double y;
    double heading;
    double kappa;
    double speed;           // 速度
    double accel;           // 加速度
    uint32_t time;          // 时间戳（毫秒）
    TraPoint() {}
    TraPoint(double x_, double y_) : x(x_), y(y_) {}
    TraPoint(double x_, double y_, double h_, double k_, double s_, double a_, double t_) :
            x(x_), y(y_), heading(h_), kappa(k_), speed(s_), accel(a_), time(t_) {} 
};


/**
 * 小车定位数据结构
*/
struct Location
{
    double x;
    double y;
    double heading;
    double kappa;
    double speed;
    double accel;
    double v_x;
    double v_y;
    double a_x;
    double a_y;
    double time;
    Location() {}
    Location(double x_, double y_) : x(x_), y(y_) {}
    // Location(Point p_, double s_, double v_x_, double v_y_, double a_x_, double a_y_) : 
    //         point(p_), speed(s_), v_x(v_x_), v_y(v_y_), a_x(a_x_), a_y(a_y_) {}

};


struct Color
{
    double r;
    double g;
    double b;
    double a;
    Color() {}
    Color(double r_, double g_, double b_, double a_) :
          r(r_), g(g_), b(b_), a(a_) {}
};


struct STMap
{
    double s_in;
    double s_out;
    double l_in;
    double l_out;
    double t_in;
    double t_out;
    STMap() {}
};

/**
 * ST 图的坐标
*/
struct STInfo
{
    double s;
    double t;
    double s_dot;
    double s_ddot;
};



#endif